#include <avr/io.h>
#include <avr/pgmspace.h>
#include <avr/eeprom.h>
#include <avr/interrupt.h>
#include <stdio.h>
#include "pll.h"
#include "util.h"
#include "main.h"

void turn_on_vco(uint8_t which) {
  if(which == VCO_HI)
    POWERCTL1_PORT |= _BV(POWERCTL1); // turn on vco
  else
    POWERCTL2_PORT |= _BV(POWERCTL2); // turn on vco
}

void set_dcoff(uint8_t which, uint8_t val) {
  if(which == VCO_HI) OCR1A = val;
  else OCR1B = val;
}

uint8_t get_dcoff(uint8_t which) {
  return (which == VCO_HI)?OCR1A:OCR1B;
}

uint8_t read_lock_pin_digital(uint8_t which) {
  if(which == VCO_HI) return bit_is_set(PLL_RFIN_PIN, PLL_RFIN);
  else return bit_is_set(PLL_IFIN_PIN, PLL_IFIN);
}

uint16_t read_lock_pin_analog(uint8_t which) {
  uint8_t i;
  uint32_t avg=0;
  
  if (which==VCO_LOW)	ADMUX = 5;
  else ADMUX = 0;
  for (i=0; i<250; i++) {
    ADCSRA |=  _BV(ADSC);
    loop_until_bit_is_clear(ADCSRA, ADSC); // wait for conversion to finish;
    avg += ADC;
  }
  return avg / 250;
}

void pll_init(void) {
  uint32_t out;
  
  ADCSRA = _BV(ADSC) | _BV(ADEN) | _BV(ADPS1) | _BV(ADPS2);
  
  PLLCLK_DDR |= _BV(PLLCLK);
  PLLDATA_DDR |= _BV(PLLDATA);
  PLLLE_DDR |= _BV(PLLLE);
  PLL_IFIN_DDR &= ~_BV(PLL_IFIN);
  PLL_RFIN_DDR &= ~_BV(PLL_RFIN);
  PLL_IFIN_PORT &= ~_BV(PLL_IFIN);
  PLL_RFIN_PORT &= ~_BV(PLL_RFIN);
  
  pll_tx(0x0, 0x2); // dont use general purpose pins, no fastlock
  pll_tx(0x0, 0x5); // dont use general purpose pins, no fastlock
  
  // setup 1MHz reference clock
  out = 3; out <<= 19; out |= (10&0x7FFF);
  pll_tx(out, 0x3); // no otherbits set: defaults
  out = 3; out <<= 19; out |= (10&0x7FFF);
  pll_tx(out, 0x0); // no otherbits set: defaults
}

void pll_tx(uint32_t data, uint8_t addr) {
  uint8_t i;
  
  if (addr > 5) return;
  
  data <<= 3;
  data |= (addr & 0x7);
  data <<= 8;
  
  PLLLE_PORT &= ~_BV(PLLLE);
  PLLDATA_PORT &= ~_BV(PLLDATA);
  PLLCLK_PORT &= ~_BV(PLLCLK);
  
  for (i=0; i<24; i++) {
    PLLCLK_PORT &= ~_BV(PLLCLK);
    if (data & 0x80000000)
      PLLDATA_PORT |= _BV(PLLDATA); 
    else
      PLLDATA_PORT &= ~_BV(PLLDATA); 
    NOP;
    PLLCLK_PORT |= _BV(PLLCLK);
    NOP; NOP;
    data <<= 1;	
  }
  PLLLE_PORT |= _BV(PLLLE);
  NOP; NOP; NOP;
  PLLLE_PORT &= ~_BV(PLLLE);
  PLLDATA_PORT &= ~_BV(PLLDATA);
  PLLCLK_PORT &= ~_BV(PLLCLK);
}

void pll_set_rcounter(uint16_t rcounter) {
  pll_tx((rcounter&0x7FFF), 0x0); // no otherbits set: defaults
  pll_tx((rcounter&0x7FFF), 0x3); // no otherbits set: defaults
}


void pll_set_freq(uint16_t rf_freq, uint8_t prescaler, uint8_t reg) {
  uint16_t N, B, A;
  uint32_t out=0;
  
  if (prescaler != 16) prescaler = 8;
  
  // Fin = N*fcomp 
  N = rf_freq; // fcomp = 1MHz
  //  N = (P*B) + A
  if (prescaler == 8) {
    B = N / 8;
    A = N % 8;
  } else {
    B = N / 16;
    A = N % 16;
  }
  
  //	printf_P(PSTR("PLL for %cF freq %dMHz & prescaler %d: B=%d A=%d\n\r"),
  //			reg==1?'R':'I',N, prescaler, B, A);
  
  if (prescaler == 16)
    out = 1; // 16 prescale 
  out <<= 15;
  out |= B; out<<= 4;
  out |= A&0xF;
  //putnum_uh(out>> 16);
  //putstring(", ");
  //putnum_uh(out);
  //putstring("\n\r");
  pll_tx(out, reg);
}

uint8_t tune_vco(uint8_t which, uint16_t freq) {
  uint8_t i, low, high;
  if (debug) printf_P(PSTR("tune_vco(%s, %d)\n\r"), which==VCO_LOW?"VCO_LOW":"VCO_HI", freq);
  
  pll_set(which, freq, 8);
  
  set_resistor(which, 0);
  turn_on_vco(which);
  
  set_dcoff(which, 5); 
  delay_ms(1000);
  if (read_lock_pin_digital(which)) {	// we cant tune any lower...???
    printf_P(PSTR("Warning: VCO range is too high!\n"));
    //		return 0;
  }
	
  set_dcoff(which, 255);	delay_ms(1000);
  
  if (! read_lock_pin_digital(which)) {	// we cant tune any higher...???
    printf_P(PSTR("Warning: VCO range is too low!\n"));
    //		return 0;
  }
  
  printf_P(PSTR("midpoint @ "));
  low = 0;
  high = 255;
  while ((low + 2) <= high) {
    i = ((uint16_t)low+(uint16_t)high)/2;
    set_dcoff(which, i);
    if (debug) printf_P(PSTR("%d, "), get_dcoff(which));
    delay_ms(1000);
    if (read_lock_pin_digital(which)) {
      delay_ms(10);
      if (read_lock_pin_digital(which)) {
	high = i;
      }
    } else {
      low = i;
    }
  }
  printf_P(PSTR("%d\n"), i);
  return i;
}

uint8_t tune_vco_band(uint8_t which, uint16_t min, uint16_t max) {
  uint16_t t, threshold;
  uint32_t avg;
  uint8_t i, j, low, high, midpt;
  
  if (debug) printf_P(PSTR("tune_vco_band(%d, %d, %d)\n"), which, min, max);

  if (min > max) {
    t = max;
    max = min;
    min = t;
  }
  midpt = tune_vco(which, (min+max)/2);

  if (midpt == 0) // start in the middle
    return 0;
  
  printf_P(PSTR("bandwidth tuning... lower bound:"));
  pll_set(which, min, 8);
  set_freq_low();

  // get high vals?
  set_resistor(which, 0);
  
  delay_ms(100);
  threshold=read_lock_pin_analog(which);
  printf_P(PSTR("max = %d\n"), threshold);
  
  low = 0;
  high = 255;
  while ((low + 2) <= high) {
    //    printf_P(PSTR("low = %d, hi=%d\n"), low, high);
    i = ((uint16_t)low+(uint16_t)high)/2;
    // set the bandwidth
    set_resistor(which, i);
    if(debug) printf_P(PSTR("set_resistor(%d, %d)\n"), which, i);
    delay_ms(1000);
    avg=read_lock_pin_analog(which);
    if(debug) printf_P(PSTR("pll=%d\n"), avg); 
    if(debug) printf_P(PSTR("%d <? %d\n"), avg, threshold); 
    if (avg < (threshold-10)) {
      high = i;
    } else {
      low = i;
    }
  }
  printf_P(PSTR("%d done!\n"), i);
#if 0      
  putstring_nl("\n\rbandwidth tuning... upper bound:");
  pll_set(which, max, 8);
  set_freq_low();

  // get high vals?
  set_resistor(which, 0);
  
  delay_ms(100);
  threshold=read_lock_pin_analog(which);
  printf_P(PSTR("threshold = %d\r\n"), threshold);
  
  low = 0;
  high = 255;
  while ((low + 2) <= high) {
    printf_P(PSTR("low = %d, hi=%d\n\r"), low, high);
    i = ((uint16_t)low+(uint16_t)high)/2;
    // set the bandwidth
    set_resistor(which, i);
    printf_P(PSTR("set_resistor(%d, %d)\r\n"), which, i);
    delay_ms(1000);
    avg=read_lock_pin_analog(which);
    printf_P(PSTR("pll=%d\r\n"), avg); 
    printf_P(PSTR("%hd <? %hd\r\n"), avg, threshold); 
    if (avg > (threshold+10)) {
      high = i;
    } else {
      low = i;
    }
  }
  printf_P(PSTR("i done!\r\n"), i);
 #endif
  set_freq_high();
  return i;
}

#if 0
uint8_t tune_vco_band(uint8_t which, uint16_t min, uint16_t max) {
	uint16_t t;
	uint8_t i, low, high, midpt;
	
	if (min > max) {
		t = max;
		max = min;
		min = t;
	}
	
	set_freq_low();

	midpt = tune_vco(which, (min+max)/2);
	if (midpt == 0) // start in the middle
		return 0;
	
	printf_P(PSTR("\n\rbandwidth tune..."));
	pll_set(which, min, 8);
	low = 0;
	high = 255;
	while ((low + 2) <= high) {
		i = ((uint16_t)low+(uint16_t)high)/2;
		// set the bandwidth
		set_resistor(which, i);
		printf_P(PSTR("%d, "), i);
		delay_ms(100);
		if (read_lock_pin_digital(which)) {
			low = i;
		} else {
			high = i;
		}
	}
	printf_P(PSTR("done!\n\r"));
	set_freq_high();
	return i;
}
#endif
